#include <CameraApi.h>

// ROS
#include <camera_info_manager/camera_info_manager.hpp>
#include <image_transport/image_transport.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/image.hpp>

// C++ system
#include <memory>
#include <string>
#include <thread>
#include <vector>

#include "camera_driver/camera_node.hpp"

namespace camera_driver
{
    // -------------------------- 构造函数实现 --------------------------
    CameraNode::CameraNode(const rclcpp::NodeOptions &options)
        : rclcpp::Node("camera_driver_node", options),
          publish_thread_running_(false)
    {
        RCLCPP_INFO(this->get_logger(), "Camera Node constructed, awaiting configuration...");

        // 1. 声明参数
        declareCameraParams();

        // 2. 初始化相机硬件
        camera_ = std::make_unique<io::Camera>(camera_params_, this->get_logger());

        // 3. 初始化图像发布器
        initImagePublisher();

        // 4. 初始化相机内参信息
        initCameraInfo();

        // 5. 注册动态参数回调
        registerDynamicParamsCallback();

        // 6. 打开相机
        camera_->open();

        // 7. 启动图像发布线程
        startPublishThread();
    }

    // -------------------------- 析构函数实现 --------------------------
    CameraNode::~CameraNode()
    {
        if (camera_)
            camera_->close();
        // 确保线程停止
        stopPublishThread();
        camera_.reset();
        RCLCPP_INFO(this->get_logger(), "Camera Node destroyed!");
    }

    // -------------------------- 私有成员函数实现 --------------------------
    void CameraNode::declareCameraParams()
    {
        // 1. 相机基础信息参数
        camera_params_.camera_name = this->declare_parameter("camera_name", "mindvision_cam");
        camera_params_.vid_pid = this->declare_parameter("vid_pid", "0x1BDA:0x0011"); // 迈德威视默认 VID/PID
        // 2. 相机控制参数
        camera_params_.exposure_ms = this->declare_parameter("exposure_ms", 10.0); // 默认10ms曝光
        camera_params_.gamma = this->declare_parameter("gamma", 1.0);              // 默认伽马1.0
        camera_params_.gain = this->declare_parameter("gain", 0.0);                // 迈德威视暂不用，留空
    }

    void CameraNode::initImagePublisher()
    {
        // 配置 QoS 策略（传感器数据默认使用低延迟的尽力而为传输）
        bool use_sensor_qos = this->declare_parameter("use_sensor_data_qos", true);
        auto qos = use_sensor_qos ? rmw_qos_profile_sensor_data : rmw_qos_profile_default;

        camera_pub_ = image_transport::create_camera_publisher(this, "image_raw", qos);
    }

    void CameraNode::initCameraInfo()
    {
        // 使用 yaml 文件加载
        std::string camera_info_url = this->declare_parameter(
            "camera_info_url", "package://camera_driver/config/camera_info.yaml");

        // 初始化相机信息管理器
        camera_info_manager_ = std::make_unique<camera_info_manager::CameraInfoManager>(
            this, camera_params_.camera_name, camera_info_url);

        // 校验 yaml 文件有效性
        if (camera_info_manager_->isCalibrated())
        {
            camera_info_msg_ = camera_info_manager_->getCameraInfo();
            RCLCPP_INFO(this->get_logger(), "Camera info loaded from yaml: %s", camera_info_url.c_str());
        }
        else
        {
            RCLCPP_WARN(this->get_logger(), "No valid camera info (uncalibrated)!");
            // 填充默认信息（避免发布空消息）
            camera_info_msg_.header.frame_id = "camera_optical_frame";
        }
    }

    void CameraNode::registerDynamicParamsCallback()
    {
        // 绑定动态参数回调函数（使用 std::bind 绑定 this 指针）
        params_callback_handle_ = this->add_on_set_parameters_callback(
            std::bind(&CameraNode::onDynamicParamsChanged, this, std::placeholders::_1));
    }

    void CameraNode::startPublishThread()
    {
        if (!publish_thread_running_)
        {
            publish_thread_running_ = true;
            publish_thread_ = std::thread{[this]()
                                          { this->publishImageLoop(); }};
            RCLCPP_INFO(this->get_logger(), "Image publish thread started.");
        }
    }

    void CameraNode::stopPublishThread()
    {
        if (publish_thread_running_)
        {
            publish_thread_running_ = false;
            if (publish_thread_.joinable())
            {
                publish_thread_.join();
                RCLCPP_INFO(this->get_logger(), "Image publish thread stopped.");
            }
        }
    }

    void CameraNode::publishImageLoop()
    {
        sensor_msgs::msg::Image ros_img_msg; // 存储读取的图像

        // 线程循环：持续读取并发布图像
        while (rclcpp::ok() && publish_thread_running_)
        {
            // 从相机读取图像（调用Camera::read() 接口）
            try
            {
                camera_->read(ros_img_msg);
                if (ros_img_msg.data.empty())
                {
                    RCLCPP_INFO(this->get_logger(), "No image data received!");
                    continue;
                }
            }
            catch (const std::exception &e) // 捕获相机读取异常
            {
                if (!rclcpp::ok())
                {
                    RCLCPP_INFO(this->get_logger(), "Program is exiting");
                    break;
                }
                RCLCPP_WARN(this->get_logger(), "Image read failed: %s, reopen camera immediately", e.what());
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
                camera_->open();
                continue;
            }

            // 设置消息头（时间戳 + 坐标系）
            ros_img_msg.header.stamp = this->now();
            ros_img_msg.header.frame_id = "camera_optical_frame";

            // 同步相机信息头（与图像时间戳一致）
            camera_info_msg_.header = ros_img_msg.header;

            // 发布图像 + 相机信息
            camera_pub_.publish(ros_img_msg, camera_info_msg_);
        }
    }

    rcl_interfaces::msg::SetParametersResult CameraNode::onDynamicParamsChanged(
        const std::vector<rclcpp::Parameter> &parameters)
    {
        rcl_interfaces::msg::SetParametersResult result;
        result.successful = true;
        result.reason = "All params set successfully";

        // 遍历参数，调用统一的 set_param 接口
        for (const auto &param : parameters)
        {
            std::string param_name = param.get_name();
            double param_value = 0.0;

            // 提取参数值（统一转为 double，子类内部处理类型）
            if (param.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE)
            {
                param_value = param.as_double();
            }
            else if (param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER)
            {
                param_value = static_cast<double>(param.as_int()); // 整数转 double（兼容曝光/增益的整数设置）
            }
            else
            {
                result.successful = false;
                result.reason = "Unsupported param type for " + param_name;
                continue;
            }

            // 调用统一接口设置参数（子类自行处理是否支持）
            if (!camera_->set_param(param_name, param_value))
            {
                result.successful = false;
                result.reason = "Failed to set param: " + param_name + " (value: " + std::to_string(param_value) + ")";
                RCLCPP_WARN(this->get_logger(), "%s", result.reason.c_str());
            }
            else
            {
                // 更新本地参数缓存
                if (param_name == "exposure_ms")
                {
                    camera_params_.exposure_ms = param_value;
                }
                else if (param_name == "gamma")
                {
                    camera_params_.gamma = param_value;
                }
                else if (param_name == "gain")
                {
                    camera_params_.gain = param_value;
                }
                RCLCPP_INFO(this->get_logger(), "Param [%s] updated to: %.2f", param_name.c_str(), param_value);
            }
        }

        return result;
    }
}

// 注册 ROS2 组件
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(camera_driver::CameraNode)